﻿// Decompiled with JetBrains decompiler
// Type: DnProcessSimulateCommands.ExportToRTR.CApExportToRTRCollisionPairExporter
// Assembly: DnProcessSimulateCommands, Version=2402.0.0.890, Culture=neutral, PublicKeyToken=null
// MVID: A885D881-AEDF-447E-852A-6F13102E089C
// Assembly location: C:\Program Files\Tecnomatix_2402\eMPower\DotNetCommands\DnProcessSimulateCommands.dll

using DnProcessSimulateCommands.ExportToRTR.YAML;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using Tecnomatix.Engineering;


namespace DnProcessSimulateCommands.ExportToRTR{

internal class CApExportToRTRCollisionPairExporter
{
  private TxCollisionPair m_collisionPair;
  private Dictionary<ITxRobot, HashSet<ITxObject>>[] m_robotOwnObjectList = new Dictionary<ITxRobot, HashSet<ITxObject>>[2]
  {
    new Dictionary<ITxRobot, HashSet<ITxObject>>(),
    new Dictionary<ITxRobot, HashSet<ITxObject>>()
  };
  private HashSet<ITxObject>[] m_robotCollideWithObjectList = new HashSet<ITxObject>[2]
  {
    new HashSet<ITxObject>(),
    new HashSet<ITxObject>()
  };
  private HashSet<ITxObject> m_handledObjects = new HashSet<ITxObject>();
  private bool m_isFirstSide = true;
  private Dictionary<string, RoadMap> m_roadMaps;
  private double? m_minimumClearance;
  private List<ITxObject> m_gupTips = new List<ITxObject>();

  private Dictionary<ITxObject, TxRobot> ObjectsWith3DOfRobot { get; } = new Dictionary<ITxObject, TxRobot>();

  private Dictionary<ITxObject, TxRobot> ChildernWith3DOfRobot { get; } = new Dictionary<ITxObject, TxRobot>();

  private HashSet<ITxObject> ObjectsWith3D { get; } = new HashSet<ITxObject>();

  private Dictionary<TxRobot, Dictionary<string, ITxObject>> RobotPrototypeLinksWith3D { get; }

  internal CApExportToRTRCollisionPairExporter(
    TxCollisionPair collisionPair,
    Dictionary<ITxObject, TxRobot> objectsWith3DOfRobot,
    Dictionary<ITxObject, TxRobot> childernWith3DOfRobot,
    HashSet<ITxObject> objectsWith3D,
    Dictionary<TxRobot, Dictionary<string, ITxObject>> robotPrototypeLinksWith3D,
    Dictionary<string, RoadMap> roadMaps,
    double? minimumClearance)
  {
    this.m_collisionPair = collisionPair;
    this.ObjectsWith3DOfRobot = objectsWith3DOfRobot;
    this.ChildernWith3DOfRobot = childernWith3DOfRobot;
    this.ObjectsWith3D = objectsWith3D;
    this.RobotPrototypeLinksWith3D = robotPrototypeLinksWith3D;
    this.m_roadMaps = roadMaps;
    this.m_minimumClearance = minimumClearance;
  }

  internal void Export()
  {
    this.ComputeRobotLists(this.m_collisionPair.FirstList, this.m_collisionPair.SecondList);
    this.m_isFirstSide = false;
    this.m_handledObjects.Clear();
    this.ComputeRobotLists(this.m_collisionPair.SecondList, this.m_collisionPair.FirstList);
    this.ExportToYaml();
  }

  private void ExportToYaml()
  {
    this.m_isFirstSide = true;
    this.ExportToYamlOneSide();
    this.m_isFirstSide = false;
    this.ExportToYamlOneSide();
  }

  private CollisionRule ExportToYamelRobotToRobotList(
    HashSet<ITxObject> robotOwnObjects,
    HashSet<ITxObject> otherListRobotOnwObjects,
    FlowFormatList<IIgnoreObjFlowForamt> firstList,
    double nearMiss)
  {
    bool flag = true;
    foreach (ITxObject robotOwnObject in robotOwnObjects)
    {
      if (otherListRobotOnwObjects.Contains(robotOwnObject))
      {
        flag = false;
        break;
      }
    }
    if (flag)
    {
      foreach (ITxObject listRobotOnwObject in otherListRobotOnwObjects)
      {
        if (robotOwnObjects.Contains(listRobotOnwObject))
        {
          flag = false;
          break;
        }
      }
    }
    CollisionRule robotToRobotList = (CollisionRule) null;
    if (flag)
    {
      FlowFormatList<IIgnoreObjFlowForamt> secondList = new FlowFormatList<IIgnoreObjFlowForamt>();
      foreach (ITxObject listRobotOnwObject in otherListRobotOnwObjects)
      {
        string linkName = CApExportToRTRYamlMgr.Instance.GetObj(listRobotOnwObject)?.Name ?? string.Empty;
        if (!string.IsNullOrEmpty(linkName))
          secondList.Add((IIgnoreObjFlowForamt) new IgnoreString(linkName));
      }
      robotToRobotList = new CollisionRule((CollisionSetsBetweenBase) new CollisionSetsBetween(firstList, secondList, true));
      robotToRobotList.SetClearance.Value = new double?(nearMiss);
    }
    return robotToRobotList;
  }

  private void AddIgonoreCollisionToYaml(
    RoadMap currentRobotRoadMap,
    FlowFormatList<IIgnoreObjFlowForamt> firstList = null,
    FlowFormatList<IIgnoreObjFlowForamt> secondList = null)
  {
    if (currentRobotRoadMap.CollisionRules == null)
    {
      currentRobotRoadMap.CollisionRules = new List<CollisionRuleBase>();
      currentRobotRoadMap.CollisionRules.Add((CollisionRuleBase) new CollisionRuleIgnore());
      currentRobotRoadMap.CollisionRules.Add((CollisionRuleBase) new CollisionRuleIgnore(second: "complete_robot"));
    }
    if (firstList == null || secondList == null)
      return;
    FlowFormatList<string> list = new FlowFormatList<string>();
    list.AddRange(currentRobotRoadMap.Assignments.Where<Assignment>((Func<Assignment, bool>) (assignment => assignment.IsWeldLoc)).Select<Assignment, string>((Func<Assignment, string>) (assignment => assignment.Target)));
    if (list.IsEmpty || firstList.IsEmpty || secondList.IsEmpty)
      return;
    currentRobotRoadMap.CollisionRules.Add((CollisionRuleBase) new CollisionRuleIgnore((CollisionSetsBetweenBase) new CollisionSetsBetween(firstList, secondList))
    {
      IgnoreCollision = {
        For = (CollisionSetForBaseClass) new CollisionSetPartialRoadmapList(list)
      }
    });
  }

  private void ExportToYamlOneSide()
  {
    Dictionary<ITxRobot, HashSet<ITxObject>> dictionary1 = this.m_isFirstSide ? this.m_robotOwnObjectList[0] : this.m_robotOwnObjectList[1];
    Dictionary<ITxRobot, HashSet<ITxObject>> dictionary2 = this.m_isFirstSide ? this.m_robotOwnObjectList[1] : new Dictionary<ITxRobot, HashSet<ITxObject>>();
    HashSet<ITxObject> itxObjectSet = this.m_isFirstSide ? this.m_robotCollideWithObjectList[0] : this.m_robotCollideWithObjectList[1];
    if (itxObjectSet.Count <= 0 && dictionary2.Count <= 0 || dictionary1.Count <= 0)
      return;
    FlowFormatList<IIgnoreObjFlowForamt> flowFormatList = new FlowFormatList<IIgnoreObjFlowForamt>();
    foreach (ITxObject txObj in itxObjectSet)
      flowFormatList.Add((IIgnoreObjFlowForamt) new IgnoreString(CApExportToRTRYamlMgr.Instance.GetObj(txObj).Name));
    foreach (KeyValuePair<ITxRobot, HashSet<ITxObject>> keyValuePair in dictionary1)
    {
      RoadMap currentRobotRoadMap;
      if (this.m_roadMaps.TryGetValue(CApExportToRTRYamlMgr.Instance.GetObj((ITxObject) keyValuePair.Key)?.Name ?? string.Empty, out currentRobotRoadMap))
      {
        FlowFormatList<IIgnoreObjFlowForamt> firstList1 = new FlowFormatList<IIgnoreObjFlowForamt>();
        FlowFormatList<IIgnoreObjFlowForamt> firstList2 = new FlowFormatList<IIgnoreObjFlowForamt>();
        foreach (ITxObject txObj in keyValuePair.Value)
        {
          string linkName = CApExportToRTRYamlMgr.Instance.GetObj(txObj)?.Name ?? string.Empty;
          if (!string.IsNullOrEmpty(linkName))
          {
            firstList1.Add((IIgnoreObjFlowForamt) new IgnoreString(linkName));
            if (this.m_gupTips.Contains(txObj))
              firstList2.Add((IIgnoreObjFlowForamt) new IgnoreString(linkName));
          }
        }
        bool checkNearMiss = TxApplication.Options.Collision.CheckNearMiss;
        double nearMiss = this.m_collisionPair.IsNearMissDefined ? this.m_collisionPair.NearMissDistance : (checkNearMiss ? TxApplication.ActiveDocument.CollisionRoot.NearMissDefaultValue : 0.1);
        IEnumerable<Dictionary<string, Connection>> source = currentRobotRoadMap.Connections.Where<Dictionary<string, Connection>>((Func<Dictionary<string, Connection>, bool>) (x => x.ContainsKey("planned")));
        if (flowFormatList.Count<IIgnoreObjFlowForamt>() > 0)
        {
          this.AddIgonoreCollisionToYaml(currentRobotRoadMap, firstList2, flowFormatList);
          CollisionRule collisionRule = new CollisionRule((CollisionSetsBetweenBase) new CollisionSetsBetween(firstList1, flowFormatList));
          collisionRule.SetClearance.Value = new double?(nearMiss);
          currentRobotRoadMap.CollisionRules.Add((CollisionRuleBase) collisionRule);
          if (this.m_minimumClearance.HasValue)
          {
            List<Dictionary<string, Connection>> list = source.ToList<Dictionary<string, Connection>>();
            currentRobotRoadMap.CollisionRules.Add((CollisionRuleBase) this.CreateMinimumClearance(collisionRule, list));
          }
        }
        HashSet<ITxObject> otherListRobotOnwObjects = (HashSet<ITxObject>) null;
        // ISSUE: explicit non-virtual call
        if (dictionary2 != null && __nonvirtual (dictionary2.TryGetValue(keyValuePair.Key, out otherListRobotOnwObjects)))
        {
          CollisionRule robotToRobotList = this.ExportToYamelRobotToRobotList(keyValuePair.Value, otherListRobotOnwObjects, firstList1, nearMiss);
          if (robotToRobotList != null)
          {
            this.AddIgonoreCollisionToYaml(currentRobotRoadMap);
            currentRobotRoadMap.CollisionRules.Add((CollisionRuleBase) robotToRobotList);
            if (this.m_minimumClearance.HasValue)
            {
              List<Dictionary<string, Connection>> list = source.ToList<Dictionary<string, Connection>>();
              currentRobotRoadMap.CollisionRules.Add((CollisionRuleBase) this.CreateMinimumClearance(robotToRobotList, list));
            }
          }
        }
      }
    }
  }

  private CollisionRule CreateMinimumClearance(
    CollisionRule collisionRule,
    List<Dictionary<string, Connection>> connections)
  {
    CollisionRule minimumClearance = new CollisionRule(collisionRule);
    minimumClearance.SetClearance.Value = this.m_minimumClearance;
    FlowFormatList<Dictionary<string, FlowFormatList<string>>> list = new FlowFormatList<Dictionary<string, FlowFormatList<string>>>();
    foreach (Dictionary<string, Connection> connection in connections)
      list.Add(new Dictionary<string, FlowFormatList<string>>()
      {
        {
          "planned",
          connection["planned"].Targets
        }
      });
    minimumClearance.SetClearance.For = (CollisionSetForBaseClass) new CollisionSetPartialRoadmapList(list);
    return minimumClearance;
  }

  private void ComputeRobotLists(TxObjectList robotList, TxObjectList objectList)
  {
    HashSet<ITxObject> itxObjectSet = new HashSet<ITxObject>();
    foreach (ITxObject robot in (Collection<ITxObject>) robotList)
      this.ComputeObjectToAddToRobotOwnList(robot);
    foreach (ITxObject itxObject in (Collection<ITxObject>) objectList)
      this.ComputeObjectToAddToRobotCollideWithList(itxObject);
  }

  private void ComputeObjectToAddToRobotOwnList(ITxObject obj)
  {
    if (!CApExportToRTRUtils.IsVisible(obj as ITxDisplayableObject))
      return;
    if (obj is TxRobot txRobot)
      this.AddKinObjToRobotOwnList(txRobot, (ITxObject) txRobot);
    else if (this.ChildernWith3DOfRobot.TryGetValue(obj, out txRobot))
    {
      if (obj is TxKinematicLink)
        this.AddLinkToRobotOwnList(obj, (ITxRobot) txRobot, this.RobotPrototypeLinksWith3D[txRobot]);
      else
        this.AddObjectToRobotOwnList(obj, (ITxRobot) txRobot);
    }
    else if (this.ObjectsWith3DOfRobot.TryGetValue(obj, out txRobot))
    {
      this.AddObjectToRobotOwnList(obj, (ITxRobot) txRobot);
      if (!(obj is TxServoGun txServoGun))
        return;
      this.AddKinObjToRobotOwnList(txRobot, (ITxObject) txServoGun);
      this.AddGunTips(txRobot, txServoGun);
    }
    else
    {
      if (!(obj is ITxObjectCollection objectCollection))
        return;
      foreach (ITxObject directDescendant in (Collection<ITxObject>) objectCollection.GetDirectDescendants((ITxTypeFilter) new TxTypeFilter(typeof (ITxObjectCollection))))
        this.ComputeObjectToAddToRobotOwnList(directDescendant);
    }
  }

  private void AddGunTips(TxRobot robot, TxServoGun servoGun)
  {
    foreach (ITxObject key in ((IEnumerable<ITxObject>) servoGun.NonCollidingEntities).ToList<ITxObject>())
    {
      if (this.ObjectsWith3DOfRobot.ContainsKey(key))
      {
        this.AddObjectToRobotOwnList(key, (ITxRobot) robot);
        this.m_gupTips.Add(key);
      }
    }
  }

  private void AddKinObjToRobotOwnList(TxRobot robot, ITxObject kinObj)
  {
    TxObjectList txObjectList1;
    if (!(kinObj is ITxObjectCollection objectCollection))
    {
      txObjectList1 = (TxObjectList) null;
    }
    else
    {
      TxTypeFilter txTypeFilter = new TxTypeFilter(new Type[2]
      {
        typeof (ITxComponent),
        typeof (TxKinematicLink)
      });
      txObjectList1 = objectCollection.GetAllDescendants((ITxTypeFilter) txTypeFilter);
    }
    if (txObjectList1 == null)
      txObjectList1 = new TxObjectList();
    TxObjectList txObjectList2 = txObjectList1;
    int count = this.m_handledObjects.Count;
    Dictionary<string, ITxObject> protoNameToLink;
    this.RobotPrototypeLinksWith3D.TryGetValue(robot, out protoNameToLink);
    foreach (ITxObject itxObject in (Collection<ITxObject>) txObjectList2)
    {
      if (this.ChildernWith3DOfRobot.ContainsKey(itxObject))
        this.AddLinkToRobotOwnList(itxObject, (ITxRobot) robot, protoNameToLink);
      else if (this.ObjectsWith3DOfRobot.ContainsKey(itxObject))
        this.AddObjectToRobotOwnList(itxObject, (ITxRobot) robot);
    }
    if (this.m_handledObjects.Count <= count)
      return;
    this.m_handledObjects.Add(kinObj);
  }

  private void AddLinkToRobotOwnList(
    ITxObject link,
    ITxRobot robot,
    Dictionary<string, ITxObject> protoNameToLink)
  {
    ITxObject itxObject = (ITxObject) null;
    // ISSUE: explicit non-virtual call
    if (protoNameToLink != null && __nonvirtual (protoNameToLink.TryGetValue(link.Name, out itxObject)))
      this.AddObjectToRobotOwnList(itxObject, robot);
    else
      this.AddObjectToRobotOwnList(link, robot);
  }

  private void AddObjectToRobotOwnList(ITxObject obj, ITxRobot robot)
  {
    if (this.m_handledObjects.Contains(obj))
      return;
    ITxObjectCollection objectCollection = robot as ITxObjectCollection;
    Dictionary<ITxRobot, HashSet<ITxObject>> dictionary = !this.m_isFirstSide ? this.m_robotOwnObjectList[1] : this.m_robotOwnObjectList[0];
    HashSet<ITxObject> itxObjectSet;
    if (!dictionary.TryGetValue(robot, out itxObjectSet))
    {
      itxObjectSet = new HashSet<ITxObject>();
      dictionary.Add(robot, itxObjectSet);
    }
    itxObjectSet.Add(obj);
    this.m_handledObjects.Add(obj);
  }

  private void ComputeObjectToAddToRobotCollideWithList(ITxObject obj)
  {
    int ofCollideWithList = this.CurrentNumberOfCollideWithList;
    this.ComputeObjectToAddToRobotCollideWithListForDescendants(obj);
    if (ofCollideWithList != this.CurrentNumberOfCollideWithList)
      return;
    this.ComputeObjectToAddToRobotCollideWithListForPredecessor(obj);
  }

  private void ComputeObjectToAddToRobotCollideWithListForDescendants(ITxObject obj)
  {
    if (this.m_handledObjects.Contains(obj) || !CApExportToRTRUtils.IsVisible(obj as ITxDisplayableObject))
      return;
    if (this.ObjectsWith3D.Contains(obj))
    {
      this.AddObjectToRobotCollideWithList(obj);
    }
    else
    {
      if (!(obj is ITxObjectCollection objectCollection))
        return;
      foreach (ITxObject directDescendant in (Collection<ITxObject>) objectCollection.GetDirectDescendants((ITxTypeFilter) new TxTypeFilter(typeof (ITxObjectCollection))))
        this.ComputeObjectToAddToRobotCollideWithListForDescendants(directDescendant);
    }
  }

  private void ComputeObjectToAddToRobotCollideWithListForPredecessor(ITxObject obj)
  {
    ITxObjectCollection collection = obj.Collection;
    if (collection == null || this.m_handledObjects.Contains((ITxObject) collection) || collection is TxPhysicalRoot)
      return;
    if (this.ObjectsWith3D.Contains((ITxObject) collection))
    {
      this.AddObjectToRobotCollideWithList((ITxObject) collection);
    }
    else
    {
      if (!(collection is TxBaseGeometryCreationObject))
        return;
      this.ComputeObjectToAddToRobotCollideWithListForPredecessor((ITxObject) collection);
    }
  }

  private void AddObjectToRobotCollideWithList(ITxObject obj)
  {
    if (this.m_isFirstSide)
      this.m_robotCollideWithObjectList[0].Add(obj);
    else
      this.m_robotCollideWithObjectList[1].Add(obj);
    this.m_handledObjects.Add(obj);
  }

  private int CurrentNumberOfCollideWithList
  {
    get
    {
      return !this.m_isFirstSide ? this.m_robotCollideWithObjectList[1].Count : this.m_robotCollideWithObjectList[0].Count;
    }
  }
}
